using BepuPhysics.Collidables;
using BepuUtilities;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.SweepTasks
{
    public struct SphereTriangleDistanceTester : IPairDistanceTester<SphereWide, TriangleWide>
    {
        public void Test(in SphereWide a, in TriangleWide b, in Vector3Wide offsetB, in QuaternionWide orientationA, in QuaternionWide orientationB, in Vector<int> inactiveLanes,
            out Vector<int> intersected, out Vector<float> distance, out Vector3Wide closestA, out Vector3Wide normal)
        {
            // 注意,这里我们从SphereTriangleCollisionTask借用了很多东西。如果你发现自己需要戏剧性地改变一些事情,可以分享更多。
            // 完全共享的主要困难是扫描测试不尊重单面性,因此一些条件会发生变化。

            // 在三角形的局部空间中工作,因为变换球体位置比变换三角形的顶点更快。
            Matrix3x3Wide.CreateFromQuaternion(orientationB, out var rB);
            Matrix3x3Wide.TransformByTransposedWithoutOverlap(offsetB, rB, out var localOffsetB);

            Vector3Wide.Subtract(b.B, b.A, out var ab);
            Vector3Wide.Subtract(b.C, b.A, out var ac);
            // localOffsetA=-localOffsetB,因此pa=triangle.A+localOffsetB。
            Vector3Wide.Add(b.A, localOffsetB, out var pa);
            Vector3Wide.CrossWithoutOverlap(ab, ac, out var localTriangleNormal);
            Vector3Wide.Dot(localTriangleNormal, pa, out var paN);

            // EdgeAB平面测试：(Pa X Ab)*(Ab X Ac)>=0
            // EdgeAC平面测试：(Ac X Pa)*(Ab X Ac)>=0
            // 请注意,这些是重心坐标的缩放版本。
            // 要将它们规格化,使三角形内一个点的权重等于1,我们只需要除以点(ab x ac,ab x ac)。
            // 换句话说,要测试第三个边平面,我们可以确保未归一化的权重都是正的,并且和为小于点(ab x ac,ab x ac)的值。
            // 如果一个点在边缘平面之外,我们知道它不在面区域或任何其他边缘区域内。但是,它可能位于相邻的顶点区域。
            // 可以通过钳制边情况来处理顶点情况。
            // 此外,请注意,对于任何查询位置,即使该点位于两个边平面之外,也只测试一条边就足够了。如果它在两个边缘平面之外,
            // 这只是意味着它将位于共享顶点上,因此夹边测试可以捕获正确的最近点。
            // 因此,在每条边上,如果点在平面之外,请缓存该边。记录外部结果的最后一个边缘将被测试。
            // (Pa X Ab)*(Ab X Ac)=(pa*ab)*(ab*ac)-(pa*ac)*(ab*ab)
            // (Ac X Pa)*(Ab X Ac)=(ac*ab)*(pa*ac)-(ac*ac)*(pa*ab)
            // (Ab X Ac)*(Ab X Ac)=(ab*ab)*(ac*ac)-(ab*ac)*(ab*ac)
            Vector3Wide.Dot(pa, ab, out var abpa);
            Vector3Wide.Dot(ab, ac, out var abac);
            Vector3Wide.Dot(ac, pa, out var acpa);
            Vector3Wide.Dot(ac, ac, out var acac);
            Vector3Wide.Dot(ab, ab, out var abab);
            var edgePlaneTestAB = abpa * abac - acpa * abab;
            var edgePlaneTestAC = abac * acpa - acac * abpa;
            var triangleNormalLengthSquared = abab * acac - abac * abac;

            var edgePlaneTestBC = triangleNormalLengthSquared - edgePlaneTestAB - edgePlaneTestAC;
            var outsideAB = Vector.LessThan(edgePlaneTestAB, Vector<float>.Zero);
            var outsideAC = Vector.LessThan(edgePlaneTestAC, Vector<float>.Zero);
            var outsideBC = Vector.LessThan(edgePlaneTestBC, Vector<float>.Zero);

            var outsideAnyEdge = Vector.BitwiseOr(outsideAB, Vector.BitwiseOr(outsideAC, outsideBC));
            Vector3Wide localClosestOnTriangle = default;
            var negativeOne = new Vector<int>(-1);
            if (Vector.EqualsAny(outsideAnyEdge, negativeOne))
            {
                // 至少有一条车道检测到三角形外的点。选择外部的一条边作为代表。
                Vector3Wide.ConditionalSelect(outsideAC, ac, ab, out var edgeDirection);
                Vector3Wide.Subtract(b.C, b.B, out var bc);
                Vector3Wide.ConditionalSelect(outsideBC, bc, edgeDirection, out edgeDirection);
                Vector3Wide.ConditionalSelect(outsideBC, b.B, b.A, out var edgeStart);

                Vector3Wide.Add(localOffsetB, edgeStart, out var negativeEdgeStartToP);
                // 如果边是AB或AC,这会做一些部分冗余的工作,但考虑到我们没有bcbc或bcpb,这是可以的。
                Vector3Wide.Dot(negativeEdgeStartToP, edgeDirection, out var negativeOffsetDotEdge);
                Vector3Wide.Dot(edgeDirection, edgeDirection, out var edgeDotEdge);
                var edgeScale = Vector.Max(Vector<float>.Zero, Vector.Min(Vector<float>.One, -negativeOffsetDotEdge / edgeDotEdge));
                Vector3Wide.Scale(edgeDirection, edgeScale, out var pointOnEdge);
                Vector3Wide.Add(edgeStart, pointOnEdge, out pointOnEdge);

                Vector3Wide.ConditionalSelect(outsideAnyEdge, pointOnEdge, localClosestOnTriangle, out localClosestOnTriangle);

            }
            if (Vector.EqualsAny(outsideAnyEdge, Vector<int>.Zero))
            {
                // P+N*(pa*N)/||N||^2=N*(pa*N)/||N||^2-(-p)
                var nScale = paN / triangleNormalLengthSquared;
                Vector3Wide.Scale(localTriangleNormal, nScale, out var offsetToPlane);
                Vector3Wide.Subtract(offsetToPlane, localOffsetB, out var pointOnFace);

                Vector3Wide.ConditionalSelect(outsideAnyEdge, localClosestOnTriangle, pointOnFace, out localClosestOnTriangle);
            }

            // Normal=Normal ize(localOffsetA-localClosestOnTriangle)=(localOffsetB+localClosestOnTriangle)/(-||localOffsetB+localClosestOnTriangle||)
            Vector3Wide.Add(localOffsetB, localClosestOnTriangle, out var localNormal);
            Vector3Wide.Length(localNormal, out var localNormalLength);
            Vector3Wide.Scale(localNormal, new Vector<float>(-1f) / localNormalLength, out localNormal);
            Matrix3x3Wide.TransformWithoutOverlap(localNormal, rB, out normal);
            Vector3Wide.Scale(normal, -a.Radius, out closestA);
            distance = localNormalLength - a.Radius;
            intersected = Vector.LessThanOrEqual(distance, Vector<float>.Zero);


        }
    }


}
